from numpy import mean, std, hstack, size, matrix, transpose
from Sensor import Sensor
import wx
import os, sys

##  BLState contains the functionality behind the BLPanel.
class BLState:

    ##  The constructor of BLState.
    #   @param[in]  main    Main wx.App derived instance.
    def __init__(self, main):
        ##  Main wx.App derived instance.
        self.main = main
        ## A matrix with boolean values, used to save the results of the checks, so they can be used for the color representation of the measurements.
        self.colours = [[] for i in range(0, len(self.main.bonyLandmarks) - len(self.main.GHJoints))]
        ##  Boolean value indicating whether a bony landmark has been checked or not.
        self.BL_CHECKED = False
        ##  Rulebase used to validate bony landmark measurements.
        self.ruleBase = self.main.config.ReadBLRulebase()
        ##  Descriptions belonging to the bony landmarks.
        self.descriptions = self.main.config.ReadDescriptions()

    ##  Returns the names of the bony landmarks as a list.
    #   @return[array]  The list of bony landmarks.
    def GetBonyLandmarkList(self):
        blList = []
        for i in range(0, len(self.main.bonyLandmarks)-len(self.main.GHJoints)):
            blList.append(self.main.bonyLandmarks[i].name)
        return blList

    ##  Calculates a matrix of relative bl vecs.
    #   @param[in]  index   The number ID of the bony landmark.
    #   @return[matrix]     The matrix containing the bl vecs.
    def __GetMeasurements(self, index):
        measurements = matrix("")
        if self.main.driver.imSensors[index] != []:
            for j in range(0, len(self.main.driver.imSensors[index])):
                blvec = self.main.driver.GetBLRelativePosition(self.main.driver.stylus[index][j],self.main.driver.imSensors[index][j])
                if j==0:
                    measurements = blvec
                else:
                    measurements = hstack((measurements, blvec))
        return measurements

    ##  Checks the five measurements of the bony landmark with index as ID.
    #   Checks if the latest measurements are not substantially different compared to the
    #   earlier measurements of the bony landmark. The average of measurements is compared to
    #   the new meausurement.
    #   @param[in] index    The number ID of the bony landmark being checked.
    def CheckSelf(self, index):
        measurements = self.__GetMeasurements(index)
        if size(measurements,1) > 1:
            checkList = [[],[],[]]
            for axis in range(0, 3):
                average = measurements[axis, :].mean()
                error = 1.5 

                for i in range(0, size(measurements,1)):
                    check = abs(measurements[axis,i] - average) < error
                    checkList[axis].append(check)

            result = []
            for i in range(0, size(measurements,1)):
                result.append(checkList[0][i] and checkList[1][i] and checkList[2][i])
            self.colours[index] = result
        else:
            self.colours[index] = [True]*size(measurements,1)

    ##  Checks all global positions on the bony landmarks using the rulebase.
    #   @return[boolean]    True if all 24 bony landmarks are true, else False.
    def CheckBonyLandmarks(self):
        # If the bony landmarks are not correct, there is no need to calculate the BLVecs, etc. and there will be no relativepositions calculated.
        for bools in self.colours:
            if len(bools) == 0:
                return False
            for bool in bools:
                if bool == False:
                    return False

        # Then calculate the global positions using these relative positions.
        globalPositions_dict = {}
        # For every bony landmark except the GH-Joints.
        for bl in range(0,len(self.main.bonyLandmarks)- len(self.main.GHJoints)):
            relativeSensor = None
            bl = self.main.bonyLandmarks[bl]
            # Find the sensor that is connected to the bony landmark.
            for sensor in self.main.sensors:
                if sensor.ID == bl.sensorID:
                    relativeSensor = sensor

            if bl.relativePosition.all() == 0:
                globalPositions_dict[bl.ID] = bl.relativePosition
            else:
                globalPositions_dict[bl.ID] = self.main.driver.RelativeToGlobal(bl.relativePosition,
                                                              relativeSensor.position,
                                                              relativeSensor.rotation)

        # Check all rules in the rulebase using these global positions.
        result_dict = {}
        for resultKey in globalPositions_dict.keys(): result_dict[resultKey] = True
        for rule in self.ruleBase:
            result_dict[rule.ID1] = result_dict[rule.ID1] and rule.IsTrue(globalPositions_dict)
            result_dict[rule.ID2] = result_dict[rule.ID2] and rule.IsTrue(globalPositions_dict)

        # Acquire a dictionary list in which the keys are the bony landmark ID's and the items are their corresponding index in a lists.
        # This is essentially mapping to make the code more generic.
        blDict = self.main.GetBonyLandmarkDict(False)

        # If the rule base returned a false value, make all five measurements orange by setting colours to false accordingly.
        for blKey in result_dict.keys():
            if result_dict[blKey] == False:
                # colours is a list, and works with the index instead of a key, so the index of the blKey is acquired through blDict.
                self.colours[blDict[blKey]] = [False for foo in range(0, len(self.colours[blDict[blKey]]))]

        # Finally returns True if ALL bony landmarks are True, otherwise False.
        for itemBool in result_dict.items():
            if itemBool == False:
                return False
        return True
    
    def StoreRawBLVecs(self):
        rawoutputlist = []
        for k in range(0, len(self.main.driver.imSensors)):
            imSensorItem = self.main.driver.imSensors[k]
            imStylusItem = self.main.driver.stylus[k]
            for i in range(0, len(imSensorItem)): # 5 stuks, als het goed is
                imStylus = imStylusItem[i]
                sensorstring = imStylus.ID.split('_')
                sample = []
                for j in imStylus.rawPosition:
                    sample.append(j)
                for j in imStylus.rawRotation:
                    sample.append(j)
                rawoutputlist.append(sensorstring[1] + '  %.2f  %.2f  %.2f  \r\n   %.2f  %.2f  %.2f  \r\n   %.2f  %.2f  %.2f  \r\n   %.2f  %.2f  %.2f  \r\n\r\n' % (sample[0], sample[1], sample[2], sample[3], sample[4],sample[5], sample[6], sample[7], sample[8], sample[9],sample[10],sample[11]))
                
                imSensor = imSensorItem[i]
                sensorstring = imSensor.ID.split('_')
                sample = []
                for j in imSensor.rawPosition:
                    sample.append(j)
                for j in imSensor.rawRotation:
                    sample.append(j)
                rawoutputlist.append(sensorstring[1] + '  %.2f  %.2f  %.2f  \r\n   %.2f  %.2f  %.2f  \r\n   %.2f  %.2f  %.2f  \r\n   %.2f  %.2f  %.2f  \r\n\r\n' % (sample[0], sample[1], sample[2], sample[3], sample[4],sample[5], sample[6], sample[7], sample[8], sample[9],sample[10],sample[11]))
                
        dlg = wx.FileDialog(self.main.gui, message="Save RAW bonylandmark vectors", defaultDir=os.getcwd(), defaultFile="",
            wildcard="FoB file (*.txt)|*.txt", style=wx.SAVE | wx.CHANGE_DIR | wx.OVERWRITE_PROMPT)
        if dlg.ShowModal() == wx.ID_OK:                
            filename = dlg.GetPath()
            fileWriter = open(filename, 'w')
            for i in rawoutputlist:
                fileWriter.write(i)
            fileWriter.close()
            self.main.driver.rawoutputlist = [] 

    ##  Resets the measurements for a bony landmark.
    #   @param[in]  index   The number ID of the bony landmark that has to be reset.
    def ResetBL(self, index):
        self.BL_CHECKED = False
        self.main.bonyLandmarks[index].relativePosition = matrix("[0.0; 0.0; 0.0]")
        self.colours[index] = []
        self.main.driver.imSensors[index] = []

    ##  Calculates and averages vector of the relative bony landmark measurement and sets it in the main bony landmark.
    #   @param[in]  index   The number ID of the bony landmark.
    def setRelativeBL(self, index):
        self.main.bonyLandmarks[index].relativePosition = self.__GetMeasurements(index).mean(1)
